"""
右转意图识别模块
"""

import numpy as np
import logging
from typing import List, Dict, Any
from scipy.optimize import minimize

class RightTurnIntentRecognizer:
    """右转意图识别器"""
    
    def __init__(self, config):
        self.config = config
        self.logger = logging.getLogger(__name__)
        
        # 右转出口区域
        self.exit_regions = config.exit_regions or self.get_default_exit_regions()
        
    def get_default_exit_regions(self):
        """获取默认的右转出口区域"""
        # 这里应该根据实际交叉口配置
        return [
            [100, 200, 150, 250],  # [x1, y1, x2, y2]
            [150, 250, 200, 300],
            [200, 300, 250, 350]
        ]
    
    def identify(self, tracks):
        """识别具有右转意图的车辆"""
        right_turn_vehicles = []
        
        for track in tracks:
            intent_score, features = self.analyze_trajectory(track)
            
            if intent_score >= self.config.intent_threshold:
                track['right_turn_intent'] = True
                track['intent_score'] = intent_score
                track['intent_features'] = features
                right_turn_vehicles.append(track)
            else:
                track['right_turn_intent'] = False
                
        return right_turn_vehicles
    
    def analyze_trajectory(self, track):
        """分析轨迹特征"""
        trajectory = track.get('trajectory', [])
        
        if len(trajectory) < self.config.min_trajectory_length:
            return 0.0, {}
        
        # 计算各个特征
        direction_consistency = self.calc_direction_consistency(trajectory)
        heading_stability = self.calc_heading_stability(track)
        exit_proximity = self.calc_exit_proximity(trajectory)
        speed_pattern = self.analyze_speed_pattern(trajectory)
        
        # 综合评分
        intent_score = (
            self.config.direction_weight * direction_consistency +
            self.config.heading_weight * heading_stability +
            self.config.proximity_weight * exit_proximity +
            self.config.speed_weight * speed_pattern
        )
        
        features = {
            'direction_consistency': direction_consistency,
            'heading_stability': heading_stability,
            'exit_proximity': exit_proximity,
            'speed_pattern': speed_pattern
        }
        
        return intent_score, features
    
    def calc_direction_consistency(self, trajectory):
        """计算方向一致性"""
        if len(trajectory) < 2:
            return 0.0
            
        # 转换为numpy数组
        traj_array = np.array(trajectory)
        
        # 计算整体移动方向
        start_pos = traj_array[0]
        end_pos = traj_array[-1]
        actual_direction = end_pos - start_pos
        
        if np.linalg.norm(actual_direction) < 1e-6:
            return 0.0
        
        # 计算与各出口方向的一致性
        max_similarity = 0.0
        for exit_region in self.exit_regions:
            # 计算出口方向
            exit_center = np.array([
                (exit_region[0] + exit_region[2]) / 2,
                (exit_region[1] + exit_region[3]) / 2
            ])
            exit_direction = exit_center - start_pos
            
            if np.linalg.norm(exit_direction) < 1e-6:
                continue
                
            # 计算余弦相似度
            actual_dir_norm = actual_direction / np.linalg.norm(actual_direction)
            exit_dir_norm = exit_direction / np.linalg.norm(exit_direction)
            
            similarity = np.dot(actual_dir_norm, exit_dir_norm)
            max_similarity = max(max_similarity, similarity)
        
        return max(0.0, max_similarity)
    
    def calc_heading_stability(self, track):
        """计算航向角稳定性"""
        if 'last_detection' not in track:
            return 0.0
            
        detection = track['last_detection']
        if 'bbox' not in detection or len(detection['bbox']) < 5:
            return 0.0
            
        # 获取航向角（旋转框的角度）
        heading_angle = detection['bbox'][4]  # 第5个元素是角度
        
        # 分析历史航向角变化（简化版）
        # 实际应该分析历史航向角序列的稳定性
        heading_variance = 0.1  #  placeholder
        
        # 右转车辆通常有特定的航向角变化模式
        stability_score = 1.0 - min(heading_variance, 1.0)
        
        return stability_score
    
    def calc_exit_proximity(self, trajectory):
        """计算出口接近度"""
        if not trajectory:
            return 0.0
            
        current_pos = np.array(trajectory[-1])
        
        # 找到最近的出口区域
        min_distance = float('inf')
        for exit_region in self.exit_regions:
            exit_center = np.array([
                (exit_region[0] + exit_region[2]) / 2,
                (exit_region[1] + exit_region[3]) / 2
            ])
            
            distance = np.linalg.norm(current_pos - exit_center)
            min_distance = min(min_distance, distance)
        
        # 距离越近，得分越高（使用指数衰减）
        max_reasonable_distance = 100.0  # 像素
        proximity_score = np.exp(-min_distance / max_reasonable_distance)
        
        return proximity_score
    
    def analyze_speed_pattern(self, trajectory):
        """分析速度模式"""
        if len(trajectory) < 3:
            return 0.5  # 中性得分
            
        traj_array = np.array(trajectory)
        
        # 计算速度序列
        displacements = traj_array[1:] - traj_array[:-1]
        speeds = np.linalg.norm(displacements, axis=1)
        
        if len(speeds) < 2:
            return 0.5
            
        # 分析速度变化模式
        # 右转车辆通常：接近交叉口时减速，转向过程中低速，离开时加速
        speed_variance = np.var(speeds)
        mean_speed = np.mean(speeds)
        
        # 简化版的速度模式分析
        if mean_speed < 0.5 and speed_variance > 0.1:
            # 低速且速度变化大，可能是转向特征
            return 0.8
        else:
            return 0.3
    
    def predict_future_trajectory(self, track, prediction_steps=10):
        """预测未来轨迹（用于意图验证）"""
        trajectory = track.get('trajectory', [])
        if len(trajectory) < 5:
            return []
            
        traj_array = np.array(trajectory)
        
        # 使用多项式拟合预测
        try:
            t = np.arange(len(traj_array))
            
            # 分别拟合x和y坐标
            x_coeffs = np.polyfit(t, traj_array[:, 0], 2)  # 二次多项式
            y_coeffs = np.polyfit(t, traj_array[:, 1], 2)
            
            # 预测未来位置
            future_t = np.arange(len(traj_array), len(traj_array) + prediction_steps)
            future_x = np.polyval(x_coeffs, future_t)
            future_y = np.polyval(y_coeffs, future_t)
            
            future_trajectory = np.column_stack([future_x, future_y])
            return future_trajectory.tolist()
            
        except:
            return []